/* Copyright (c) 2011 Alexandre Défossez, Timothée Lacroix
 * This file is part of HINAPT.
 *
 * HINAPT is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * HINAPT is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with HINAPT.  If not, see <http://www.gnu.org/licenses/>.
 */



#include "tracker.hpp"

using namespace sf;

namespace {
  /*!
    Interdiction de s'approcher à moins d'autant de pixel des limites de l'image
  */
  const int border=10;
  /*!
    Seuils utilisés pour déterminer quand un œil est perdu, un pour chaque.
    En 0 : œil gauche, en 1 : œil droit.
  */
  double lostThreshold[2] = {1e-5, 1e-5};
  /*!
    Un œil doit être déclaré perdu pendant au moins autant de secondes pour
    que l'algorithme de détection soit relancé. Évite de tout relancer parce
    que l'utilisateur a cligné des yeux.
  */
  const double lostMinTime = 0.3;
  /*!
    Si le mouvement est inférieur à autant de pixels, le mouvement est ignoré.
    0 = désactivé
  */
  const double moveThreshold = 0.;
  /*!
    Si la correction apportée par le suivi affine est inférieur à autant
    de pixels, cette correction est ignorée. 0 = désactivé
  */
  const double affineThreshold = 0.;
  /*!
    Distance minimale, en pixels, entre les deux yeux pour ne pas les déclarer
    perdu. Cela évite que le même œil soit suivi deux fois.
  */
  const double minimalEyeDistance = 5;

  /*!
    Si vrai, affiche l'image de la caméra dans une autre fenêtre. Cette fenêtre
    s'ouvrira même pour faux, mais ne sera pas peinte.
  */
  const bool showFace = true;

  /*!
    Résolution en largeur de la caméra, en pixel
  */
  int cameraWidth = 640;
  /*!
    Résolution en hauteur de la caméra, en pixel
  */
  int cameraHeight = 480;
  /*!
    Hauteur de l'écran
  */
  double screenHeight = 22;
  /*!
    Angle de la caméra avec le plan de l'écran
  */
  double camAngle = 0.2;
  /*!
    Distance entre les yeux
  */
  double eyesDistance = 6.5;
  /*!
    Caméra à utiliser, -1 = défault
  */
  int cameraIndex = -1;
  }

sf::Clock globalClock;

int realMain();
int parseCommandLine(int argc, char* argv[]);

/*! Fonction d'entrée du programme, gère les erreurs */
int main(int argc, char* argv[]){
  try{
    return parseCommandLine(argc, argv);
  }
  catch(const std::runtime_error& e){
    std::cerr << "FATAL ERROR : " << e.what() << std::endl;
    return EXIT_FAILURE;
  }

}

void printHelp(){
  printf("tracker [OPTION]\n\
  -h\tshow this help\n\
  -c\tcamera index (int)\n\
  -s\tscreen height (double)\n\
  -d\tdistance between the eyes (double)\n\
  -H\tcamera height in px (int)\n\
  -W\tcamera width in px (int)\n\
  -l\tleft lost threshold (double)\n\
  -r\tright lost threshold (double)\n");

}

int parseCommandLine(int argc, char* argv[]){
  char c;
  while ((c = getopt (argc, argv, "c:s:d:H:W:l:r:h")) != -1)
    switch (c)
    {
      case 'h':
        printHelp();
        return 0;
      case 'c':
        cameraIndex = atoi(optarg);
        break;
      case 's':
        screenHeight = atof(optarg);
        break;
      case 'd':
        eyesDistance = atof(optarg);
        break;
      case 'H':
        cameraHeight = atoi(optarg);
        break;
      case 'W':
        cameraWidth = atoi(optarg);
        break;
      case 'l':
        lostThreshold[0] = atof(optarg);
        break;
      case 'r':
        lostThreshold[1] = atof(optarg);
        break;

      case '?':
        fprintf(stderr, "Unexpected option -%c or missing argument\n",
                optopt);
        return 1;
      default:
        return EXIT_FAILURE;
  }
  return realMain();

}


/*! Vraie fonction d'entré du programme. */
int realMain(){
    /* Initialisation des images */
    Mat img;
    Mat oldImg;

    /* Initialisation de la caméra */
    VideoCapture capture(-1);
    Assert(capture.isOpened(), errors::camera_failure);
    capture.set(CV_CAP_PROP_FRAME_WIDTH, cameraWidth);
    capture.set(CV_CAP_PROP_FRAME_HEIGHT, cameraHeight);
    capture.grab();
    getImage(capture, img);
    oldImg = img;


    /* Initialisation des variables de suivi */

    cv::Rect boundaries(border, border,
                         cameraWidth-border-1, cameraHeight-border-1);

    AffineTracker atrack[2];
    double atrackError[2];

    HeadPosition hp(22, 6.5, 0.2, cameraWidth, cameraHeight); //alex : 22 6.5 0.2    timothée : 45 6.5 0.4

    Mat A; //matrice d'aide pour le suivi affine
    Mat Mtmp; //utilisé comme variable temporaire

    std::vector<Point2f> eyes;
    eyes.push_back(Point2f(100, 100));
    eyes.push_back(Point2f(150, 100));
    std::vector<cv::Rect> eyesROI(2);

    hp.setOriginalPositions(eyes);

    double imgTime = 0;
    Clock clock; // pour la mesure du framerate

    /* Initialisation des variables de détection */
    EyeDetector detector;
    double isLost = true;
    Clock lastSuccess;

    /* Initialisation des variables de dessins pour la SFML */
    std::vector<Shape> carres;
    carres.resize(2, Shape::Rectangle(0,0, 10, 10, Color(255,0,0,255)));
    Sprite sprite;
    Image cam;


    /* Communication inter-threads */
    HinaptState hinaptState = {true};
    Point3d hpos;  //position de la tête dans l'espace
    Mutex AppLock;
    ThQueue<std::pair<Point3d, double> > headPosQueue;
    ThQueue<std::pair<Mat, double> > imgQueue;
    imgQueue.setMaxSize(1);
    bool imgThreadStop = false;


    /* Création des fenêtres */
    RenderWindow AppGL(sf::VideoMode::GetMode(0), "HINAPT GL", sf::Style::None);
    RenderWindow App(sf::VideoMode(cameraWidth, cameraHeight), "HINAPT");



    /* Initialisation d'OpenGL */

    GLuint targetTexture = 0;

    AppGL.UseVerticalSync(true);
    AppGL.PreserveOpenGLStates(true);

    /* Initialisation des valeurs d'effacement
    pour les tampons de couleur et de profondeur */
    glClearDepth(1.f);
    glClearColor(0.f, 0.f, 0.f, 0.f);

    /* Activation de la lecture et de l'écriture dans le tampon de profondeur */
    glEnable(GL_DEPTH_TEST);
    glDepthMask(GL_TRUE);

    /* Anti-aliasing et paramétrage du tracé de lignes */
    glEnable (GL_LINE_SMOOTH);
    glEnable (GL_BLEND);
    glBlendFunc (GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
    glHint (GL_LINE_SMOOTH_HINT, GL_DONT_CARE);
    glLineWidth (1.5);

    /* Mise en place d'une projection perspective */
    glMatrixMode(GL_PROJECTION);
    glLoadIdentity();
    gluPerspective(90.f, 1.f, 1.f, 500.f);

    /* Chargement de la texture des cibles */
    glEnable(GL_TEXTURE_2D);
    glBindTexture(GL_TEXTURE_2D, targetTexture);

    {
      Image img;
      img.LoadFromFile("target.png");
      glGenTextures(1, &targetTexture);
      glBindTexture(GL_TEXTURE_2D, targetTexture);
      gluBuild2DMipmaps(GL_TEXTURE_2D, GL_RGBA, img.GetWidth(),
                        img.GetHeight(), GL_RGBA, GL_UNSIGNED_BYTE,
                        img.GetPixelsPtr());
      glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
      glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER,
                      GL_LINEAR_MIPMAP_LINEAR);
    }

    AppGL.SetActive(false);

    /* Création et lancement des threads */

    GLThread glThread(AppGL, AppLock, headPosQueue, targetTexture,hinaptState);
    glThread.Launch();

    CameraThread camThread(capture, imgQueue, imgThreadStop);
    camThread.Launch();

    /* Début détection/suivi */

    AppLock.Lock();

    while (AppGL.IsOpened()){
        AppLock.Unlock();

        std::pair<Mat, double> pair;

        // Si on a pas d'image a traiter, on dort un peu.
        while(!imgQueue.pop(pair)){
          Sleep(1./60.);
        }
        img = pair.first;
        imgTime = pair.second;

        clock.Reset();
        vector<Point2f> tmp = eyes;

        if (!isLost){ // si on est en suivi

          if (!trackMulti(eyes,oldImg, img)) // suivi par translation
            eyes = tmp;

          #pragma omp parallel for
          for(int i=0; i<2; i++){ // suivi affine parallélisé
            Mat D = A.clone();
            Vec2d d;

            atrackError[i] = atrack[i].track(img, D, d, eyes[i]);
            atrackError[i] = atrackError[i];
            if (atrackError[i] >= 0) {
              Point2f newPos(d[0]+ atrack[i].getPoint().x, d[1] + atrack[i].getPoint().y);
              if (distance(newPos, eyes[i])>=affineThreshold)
                eyes[i] = newPos;
              inbound(eyes[i], boundaries);
            }
          }

          double success = true;
          for (int i=0; i<2; i++){
            if ((atrackError[i] > lostThreshold[i])||(atrackError[i]<0)){
              printf("not success %d %f\n", i,atrackError[i]);
              success = false;
              break;
            }
            printf("Eye %d : %e\n", i ,atrackError[i]);
          }
          if(distance(eyes[0], eyes[1])<minimalEyeDistance)
            success = false;
          if(!success){
            if (lastSuccess.GetElapsedTime()>lostMinTime){
              isLost = true;
              AppLock.Lock();
              hinaptState.hasBeenLost = true;

              /* Ici, on réduit le tampon d'images à 1, car le traitement
              est beaucoup plus long lors du suivi donc on veut toujours traiter
              la dernière image pour ne pas prendre du retard */
              imgQueue.setMaxSize(1);
              AppLock.Unlock();
              printf("LOST !!!\n");
            }
          }
          else
            lastSuccess.Reset();
          if ((distance<Point2f>(tmp[0], eyes[0]) < moveThreshold) &&
              (distance<Point2f>(tmp[1], eyes[1]) < moveThreshold)){

            eyes = tmp;
          } else {
            oldImg = img;
          }
      } else { // on cherche les yeux
        lastSuccess.Reset();
        if (detector.find(img, eyesROI[0], eyesROI[1])
            && inbound(eyes[0]=center(eyesROI[0]), boundaries)
            && inbound(eyes[1]=center(eyesROI[1]), boundaries)){

          Mtmp = Mat(img, eyesROI[0]);
          atrack[0].update(Mtmp, eyes[0]);
          Mtmp = Mat(img, eyesROI[1]);
          atrack[1].update(Mtmp, eyes[1]);

          oldImg = img;
          isLost = false;

          /* Retour au tampon normal de 3 images */
          imgQueue.setMaxSize(3);

          hp.setOriginalPositions(eyes);
          printf("Gotcha in %.2f seconds\n", lastSuccess.GetElapsedTime());
          lastSuccess.Reset();
        } else
          eyes = tmp;
      }

      int framerate_theo = static_cast<int>(1.f/clock.GetElapsedTime());

      printf("temps theo : %d\n", framerate_theo);

      if (!isLost){// on envoie la position à GLThread
        hpos = hp(eyes, A);
        std::pair<Point3d, double> pair(hpos, imgTime);
        headPosQueue.push(pair);
      }

      AppLock.Lock();

      if (showFace){
        MatToSFML(img, cam);

        sprite.SetImage(cam);
        sprite.SetPosition(0,0);
        App.Clear();
        App.Draw(sprite);
        for(int i=0;i<(int)carres.size();i++){
            carres[i].SetPosition(eyes[i].x-5, eyes[i].y-5);
            App.Draw(carres[i]);
        }

        App.Display();
      }
    }
    AppLock.Unlock();
    imgThreadStop = true;
    glThread.Wait();
    camThread.Wait();
    return EXIT_SUCCESS;
}





